/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file dataToXML.cxx
	\brief class implemenation XML generator

									  \author Jack Elston
						 $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include <xercesc/framework/MemBufInputSource.hpp>
#include <xercesc/framework/Wrapper4InputSource.hpp>

#include "dataToXML.h"

#include "telemetry_limited.hxx"
#include "link_status.hxx"
#include "task_status.hxx"
#include "task.hxx"
#include "capabilities.hxx"
#include "atmospheric_data.hxx"
#include "flight_plan.hxx"
#include "wind_data.hxx"
#include "stream_subscription.hxx"

XERCES_CPP_NAMESPACE_USE

using std::auto_ptr;
using namespace CUIntegration;
using xml_schema::flags;

	/*<-------Defines--------->*/
	/*<------End Defines------>*/

	/*<---Global Variables---->*/
		/*! \brief determines level of output to user. */
		extern bool verbose;
	/*<-End Global Variables-->*/

/************************************************************\
|                  DataToXML_Parser::init                    |
\************************************************************/
/*!
	\brief the initializing function for the XML generating thread.
	\param shm shared memory struct between the gateway threads.
	\pre instance of DataToXML_Parser has been created
	\post the XML utils have been intialized and the inetTxFifo
	 and SAVRxFifo have been opened
	\return TRUE on successful completion of initialization, FALSE otherwise
 */
DataToXML_Parser::DataToXML_Parser(String name) : ThreadedPipe(name)
{
	try {
		xercesc::XMLPlatformUtils::Initialize();
	}
	catch (const XMLException& toCatch) {
		perror("DataToXML_Parser::init() - could not initialize xerces");
	}
}

ThreadedPipe::commMode DataToXML_Parser::mode() {return ThreadedPipe::TX;};

/************************************************************\
|                 DataToXML_Parser::update                   |
\************************************************************/
/*!
	\brief function called periodically by the thread.
	\pre instance of DataToXML_Parser has been created and initialized
	\post any data in the SAVRx will be parsed, converted to XML and
	 output to the inetTxFifo
 */
void DataToXML_Parser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;				// read fd set
	struct timeval timeout;

	FD_ZERO(&readFDs);				// zero read fd set
	FD_ZERO(&writeFDs);				// zero read fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);			// add fifo

	fifo_FDs[OUTBOARD_TX] > fifo_FDs[INBOARD_RX] ? maxFD = fifo_FDs[OUTBOARD_TX] : maxFD = fifo_FDs[INBOARD_RX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("DataToXML_Parser::update() select");
				execRate = -1;
			} else
				cout << "DataToXML_Parser::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(INBOARD_RX);
			if( n < 0 && errno != EAGAIN)
				perror("DataToXML_Parser::update() - error reading from fifo ");
			else {
				if(n>0) {

				 uint16_t length = CMD_SIZE(fifo_msg.data);
				 if( length != fifo_msg.size) {
						cout << getName() << "::update() - error, length of command != length of buffer" << endl
								 << "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

						if( length < 1000 && length <= fifo_msg.size ) {
							 if( C_calcCheckSum((uint8_t *)buf) == 0 )
								cout << "\tCMD: is valid" << endl;
							 else
								cout << "\tCMD: is NOT valid" << endl;
						}

						return;
				 }

					Command cmd((uint8_t*)fifo_msg.data);

					if( !cmd.isValid() )
						cout << "DataToXML::update() - error, invalid command in rxfifo"<< endl << cmd ;
					else
						parse(cmd, fifo_msg.src);
				}
			}
		}
	}
}

void DataToXML_Parser::parse(const Command &cmd, in_addr address) 
{
	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);
	Client * a_client = shmPtr->clients.getClient(address);
	if (a_client == NULL) return;
	int n;

	switch(cmd.cmd()) {
		case CMD_CAPABILITY: {
				VehicleInfo_t info;
				CopyBufToStruct( dataPtr, info);
				dataPtr +=sizeof(VehicleInfo_t);

				Capabilities::Capabilities_type capability(info.id);

				SystemCharacter_t * a_system;

				for(unsigned int i=0; i<((cmd.size()-sizeof(VehicleInfo_t))/sizeof(SystemCharacter_t)); i++) {
					a_system = &(((SystemCharacter_t*)dataPtr)[i]);
				  Capabilities::System xml_system(Capabilities::ID::ID(""));
					switch((a_system->id) & 0xF0) {
						case CMD_SERVO:
							xml_system.ID(
									Capabilities::ID::ID("Servo"));
							capability.System().push_back(xml_system);
							break;
						case CMD_IMU:
							xml_system.ID(
									Capabilities::ID::ID("IMU"));
							if(a_system->streams > 0) {
								Capabilities::AvailableStreams xml_streams;
								for(int j=0; j< 16; j++) {
									if(((a_system->streams) >> j) & 0x1)
										switch (j+1) {
											case 1:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Angles"));
												break;
											case 2:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Rates"));
												break;
											case 3:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Accelerations"));
												break;
										}
								}
								xml_system.AvailableStreams(xml_streams);
							}
							capability.System().push_back(xml_system);
							break;
						case CMD_AIR_DATA:
							xml_system.ID(
									Capabilities::ID::ID("Air Data"));
							if(a_system->streams > 0) {
								Capabilities::AvailableStreams xml_streams;
								for(int j=0; j< 16; j++) {
									if(((a_system->streams) >> j) & 0x1)
										switch (j+1) {
											case 1:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Wind"));
												break;
										}
								}
								xml_system.AvailableStreams(xml_streams);
							}
							capability.System().push_back(xml_system);
							break;
						case CMD_GPS:
							xml_system.ID(
									Capabilities::ID::ID("GPS"));
							if(a_system->streams > 0) {
								Capabilities::AvailableStreams xml_streams;
								for(int j=0; j< 16; j++) {
									if(((a_system->streams) >> j) & 0x1)
										switch (j+1) {
											case 1:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Latitude and Longitude"));
												break;
											case 2:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Altitude"));
												break;
											case 3:
												xml_streams.Stream().push_back(
													Capabilities::Stream("UTC"));
												break;
											case 4:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Track"));
												break;
											case 5:
												xml_streams.Stream().push_back(
													Capabilities::Stream("SOG"));
												break;
											case 6:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Accuracy"));
												break;
											case 7:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Telemetry"));
												break;
										}
								}
								xml_system.AvailableStreams(xml_streams);
							}
							capability.System().push_back(xml_system);
							break;
						case CMD_COMM:
							xml_system.ID(
									Capabilities::ID::ID("Communications"));
							if(a_system->streams > 0) {
								Capabilities::AvailableStreams xml_streams;
								for(int j=0; j< 16; j++) {
									if(((a_system->streams) >> j) & 0x1)
										switch (j+1) {
											case 1:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Link Status"));
												break;
										}
								}
								xml_system.AvailableStreams(xml_streams);
							}
							capability.System().push_back(xml_system);
							break;
						case CMD_H_AND_S:
							xml_system.ID(
									Capabilities::ID::ID("Health and Status"));
							capability.System().push_back(xml_system);
							break;
						case CMD_PAYLOAD:
							xml_system.ID(
									Capabilities::ID::ID("Payload"));
							if(a_system->streams > 0) {
								Capabilities::AvailableStreams xml_streams;
								for(int j=0; j< 16; j++) {
									if(((a_system->streams) >> j) & 0x1)
										switch (j+1) {
											case 1:
												xml_streams.Stream().push_back(
													Capabilities::Stream("PTH"));
												break;
											case 2:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Sonde"));
												break;
										}
								}
								xml_system.AvailableStreams(xml_streams);
							}
							capability.System().push_back(xml_system);
							break;
						case CMD_AUTOPILOT:
							xml_system.ID(
									Capabilities::ID::ID("Autopilot"));
							
							if(a_system->commands > 0) {
								Capabilities::AcceptedCommands xml_commands;
								for(int j=0; j< 16; j++) {
									if(((a_system->commands) >> j) & 0x1)
										switch (j+1) {
											case 1:

												xml_commands.Command().push_back(
													Capabilities::Command("Go to Waypoint"));
												break;
											case 2:
												xml_commands.Command().push_back(
														Capabilities::Command("Flight Plan"));
												break;
											case 4:
												xml_commands.Command().push_back(
														Capabilities::Command("Autopilot Rates"));
												break;
										}
								}
								xml_system.AcceptedCommands(xml_commands);
							}

							if(a_system->streams > 0) {
								Capabilities::AvailableStreams xml_streams;
								for(int j=0; j< 16; j++) {
									if(((a_system->streams) >> j) & 0x1)
										switch (j+1) {
											case 5:
												xml_streams.Stream().push_back(
													Capabilities::Stream("Flight Plan"));
												break;
											case 3:
												xml_streams.Stream().push_back(
														Capabilities::Stream("Telemetry"));
												break;
										}
								}
								xml_system.AvailableStreams(xml_streams);
							}

							capability.System().push_back(xml_system);
							break;
						case CMD_FLIGHT_COMPUTER:
							xml_system.ID(
									Capabilities::ID::ID("FlightComputer"));
							if(a_system->commands > 0) {
								Capabilities::AcceptedCommands xml_commands;
								for(int j=0; j< 16; j++) {
									if(((a_system->commands) >> j) & 0x1)
										switch (j+1) {
											case 3:
												xml_commands.Command().push_back(
													Capabilities::Command("Coordinate"));
												break;
											case 4:
												xml_commands.Command().push_back(
													Capabilities::Command("Custody"));
												break;
											case 5:
												xml_commands.Command().push_back(
													Capabilities::Command("Track"));
												break;
											case 6:
												xml_commands.Command().push_back(
													Capabilities::Command("Phase"));
												break;
											case 7:
												xml_commands.Command().push_back(
													Capabilities::Command("Search"));
												break;
											case 8:
												xml_commands.Command().push_back(
													Capabilities::Command("Return"));
												break;
											case 9:
												xml_commands.Command().push_back(
													Capabilities::Command("Formation"));
												break;
											case 10:
												xml_commands.Command().push_back(
													Capabilities::Command("Chain"));
												break;
											case 11:
												xml_commands.Command().push_back(
													Capabilities::Command("Chain Gains"));
												break;
											case 12:
												xml_commands.Command().push_back(
													Capabilities::Command("Orbit Values"));
												break;
										}
								}
								xml_system.AcceptedCommands(xml_commands);
							}
							capability.System().push_back(xml_system);
              break;
					}
				}

				xml_schema::namespace_infomap map;
				map[""].name = "http://Capabilities.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\capabilities.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				Capabilities::Capabilities (*ft, capability, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

				} break;
		case CMD_GPS_TELEM: {
				GPSTelem_t telemetry;
				CopyBufToStruct( dataPtr, telemetry);

				Telemetry::Telemetry_type telem( a_client->name.c_str(), 
						Telemetry::GPS::GPS(),
						Telemetry::AirData::AirData(),
						Telemetry::InertialMeasurements::InertialMeasurements(),
						Telemetry::ControlLoops::ControlLoops(),
						Telemetry::SystemStatus::SystemStatus() );

				telem.GPS().Latitude().set(telemetry.pos.lat);
				telem.GPS().Longitude().set(telemetry.pos.lon);
				telem.GPS().Track().set(telemetry.track);
				telem.GPS().Height().set(telemetry.pos.alt);
				telem.GPS().SOG(telemetry.sog);
				telem.GPS().UTCHours().set(telemetry.utc.hours);
				telem.GPS().UTCMinutes().set(telemetry.utc.minutes);
				telem.GPS().UTCSeconds().set(telemetry.utc.seconds);

				xml_schema::namespace_infomap map;
				map[""].name = "http://Telemetry.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\telemetry_limited.xsd";

				//Telemetry::Telemetry(std::cout,telem,map);

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				Telemetry::Telemetry (*ft, telem, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

				} break;
		case CMD_TELEM: {
				Telemetry_t telemetry;
				CopyBufToStruct( dataPtr, telemetry);

				Telemetry::Telemetry_type telem( a_client->name.c_str(), 
						Telemetry::GPS::GPS(),
						Telemetry::AirData::AirData(),
						Telemetry::InertialMeasurements::InertialMeasurements(),
						Telemetry::ControlLoops::ControlLoops(),
						Telemetry::SystemStatus::SystemStatus() );

				telem.GPS().Latitude(telemetry.gps.pos.lat);
				telem.GPS().Longitude(telemetry.gps.pos.lon);
				telem.GPS().Track(telemetry.gps.track);
				telem.GPS().Height(telemetry.gps.pos.alt);
				telem.GPS().SOG(telemetry.gps.sog);
				telem.GPS().UTCHours(telemetry.gps.utc.hours);
				telem.GPS().UTCMinutes(telemetry.gps.utc.minutes);
				telem.GPS().UTCSeconds(telemetry.gps.utc.seconds);

				telem.AirData().BarometricAlt(telemetry.airdata.alt);
				telem.AirData().TrueAirSpeed(telemetry.airdata.tas);

				telem.InertialMeasurements().RollAngle(telemetry.angles.roll/1000.0*180.0/M_PI);
				telem.InertialMeasurements().PitchAngle(telemetry.angles.pitch/1000.0*180.0/M_PI);
				telem.InertialMeasurements().YawAngle(telemetry.angles.yaw/1000.0*180.0/M_PI);

				telem.ControlLoops().CommandedWaypoint(telemetry.navigation.waypt);

				telem.SystemStatus().MainInputVoltage(telemetry.system.twelveV/1000.0);
				telem.SystemStatus().ServoInputVoltage(telemetry.system.fiveV/1000.0);
				telem.SystemStatus().AvailableForCommand(telemetry.system.availableForCommand);

				xml_schema::namespace_infomap map;
				map[""].name = "http://Telemetry.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\telemetry_limited.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				// look into defining XSD_USE_LCP if this line fails with a utf8 error
				Telemetry::Telemetry(*ft, telem, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_SONDE: {
				Sonde_t sonde;
				CopyBufToStruct( dataPtr, sonde);

				AtmosphericData::AtmosphericData_type air_data(a_client->name.c_str());
				AtmosphericData::PTHSensor pth_sensor(
						0,
						AtmosphericData::GPS::GPS(),
						AtmosphericData::AirData::AirData());

				pth_sensor.GPS().UTCHours().set(sonde.gps.utc.hours);
				pth_sensor.GPS().UTCMinutes().set(sonde.gps.utc.minutes);
				pth_sensor.GPS().UTCSeconds().set(sonde.gps.utc.seconds);
				pth_sensor.GPS().Latitude().set(sonde.gps.pos.lat);
				pth_sensor.GPS().Longitude().set(sonde.gps.pos.lon);
				pth_sensor.GPS().Track().set(sonde.gps.track);
				pth_sensor.GPS().Height().set(sonde.gps.pos.alt);
				pth_sensor.GPS().SOG().set(sonde.gps.sog);

				pth_sensor.SensorID(sonde.id);
				pth_sensor.AirData().Pressure(sonde.pth.pressure);
				pth_sensor.AirData().Temperature(sonde.pth.temp);
				pth_sensor.AirData().InternalTemperature(sonde.pth.internal_temp);
				pth_sensor.AirData().RelativeHumidity1(sonde.pth.rh1);
				pth_sensor.AirData().RelativeHumidity2(sonde.pth.rh2);

				air_data.PTHSensor().push_back(pth_sensor);

				xml_schema::namespace_infomap map;
				map[""].name = "http://AtmosphericData.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\atmospheric_data.xsd";

//        AtmosphericData(cout,air_data,map);
				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				AtmosphericData::AtmosphericData (*ft, air_data, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_WIND: {
				WindData_t wind;
				CopyBufToStruct( dataPtr, wind);

				WindData::WindData_type wind_data(a_client->name.c_str());
				WindData::WindSensor wind_sensor(0,
						WindData::GPS(),
						WindData::WindComponents());

				wind_sensor.GPS().UTCHours().set(wind.gps.utc.hours);
				wind_sensor.GPS().UTCMinutes().set(wind.gps.utc.minutes);
				wind_sensor.GPS().UTCSeconds().set(wind.gps.utc.seconds);
				wind_sensor.GPS().Latitude().set(wind.gps.pos.lat);
				wind_sensor.GPS().Longitude().set(wind.gps.pos.lon);
				wind_sensor.GPS().Track().set(wind.gps.track);
				wind_sensor.GPS().Height().set(wind.gps.pos.alt);
				wind_sensor.GPS().SOG().set(wind.gps.sog);

				wind_sensor.WindComponents().South().set(wind.south);
				wind_sensor.WindComponents().West().set(wind.west);

				wind_data.WindSensor().push_back(wind_sensor);

				xml_schema::namespace_infomap map;
				map[""].name = "http://WindData.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\wind_data.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				WindData::WindData (*ft, wind_data, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		/*case CMD_TRACK: {  //FIXME -- might throw exception see logs from 2010-06-02
			int nVehicles = (cmd.size()/sizeof(VehicleInfo_t));

			VehicleInfo_t * vehicle = (VehicleInfo_t *)dataPtr;
			VehicleInfo_t myVehicles[nVehicles];

			Client * dest_client = shmPtr->clients.getClient(fifo_msg.dest);
			if( !dest_client ) return;

			Task::Task_type task("ID",
					"Track",
					dest_client->name.c_str());

			for(int i = 0; i < nVehicles; i++) 
			{
				myVehicles[i] = vehicle[i];
				Task::Node xml_node;
				xml_node.ID(myVehicles[i].id);
				task.Node().push_back(xml_node);
			}

			xml_schema::namespace_infomap map;
			map[""].name = "http://Task.CUIntegration.com";
			map[""].schema = "..\\Schemas\\CU\\task.xsd";

			auto_ptr <MemBufFormatTarget> ft;
			ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
			Task::Task (*ft, task, map);

			if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
				memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
				fifo_msg.size = ft->getLen();
				n=writeFifo(OUTBOARD_TX);
				if( n != (int)ft->getLen() && errno != EAGAIN)
					perror("DataToXML::update() - error writing to fifo ");
			}
			else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_ORBIT_VALUES: {
			OrbitValues_t * orbit_values = (OrbitValues_t *)dataPtr;

			Client * dest_client = shmPtr->clients.getClient(fifo_msg.dest);
			if( !dest_client ) return;

			Task::Task_type task("ID",
					"OrbitValues",
					dest_client->name.c_str());

			task.Values(Task::Values());

			task.Values()->Radius() = orbit_values->rad;
			task.Values()->Altitude() = orbit_values->alt;
			task.Values()->Waypoint() = orbit_values->wp;
			task.Values()->Direction() = orbit_values->dir;
			task.Values()->OffsetNorth() = orbit_values->offset[0];
			task.Values()->OffsetEast() = orbit_values->offset[1];

			xml_schema::namespace_infomap map;
			map[""].name = "http://Task.CUIntegration.com";
			map[""].schema = "..\\Schemas\\CU\\task.xsd";

			auto_ptr <MemBufFormatTarget> ft;
			ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
			Task::Task (*ft, task, map);

			if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
				memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
				fifo_msg.size = ft->getLen();
				n=writeFifo(OUTBOARD_TX);
				if( n != (int)ft->getLen() && errno != EAGAIN)
					perror("DataToXML::update() - error writing to fifo ");
			}
			else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_FP: { 

			int nPoints = (cmd.size()/sizeof(Waypt_t));

			// our format of waypoints
			Waypt_t * waypoint = (Waypt_t *)dataPtr;
			Waypt_t myWaypoints[nPoints];

			String task_type;
			if (nPoints <= 2) 
				task_type = "Loiter";
			else if (fabs(myWaypoints[nPoints-1].orbitRad) > 0) 
				task_type = "Path";
			else task_type = "Loop";

			Client * dest_client = shmPtr->clients.getClient(fifo_msg.dest);
			if( !dest_client ) return;

			Task::Task_type task("ID",
					task_type.c_str(),
					dest_client->name.c_str());

			for(int i = 0; i < nPoints; i++) 
			{
				myWaypoints[i] = waypoint[i];
				int wpInd      = myWaypoints[i].number; 

				Task::Waypoint xml_waypoint;

				xml_waypoint.Latitude() = myWaypoints[i].pos.lat;
				xml_waypoint.Longitude() = myWaypoints[i].pos.lon;
				xml_waypoint.SequenceNumber() = wpInd;

				task.Waypoint().push_back(xml_waypoint);
			}

			xml_schema::namespace_infomap map;
			map[""].name = "http://Task.CUIntegration.com";
			map[""].schema = "..\\Schemas\\CU\\task.xsd";

			auto_ptr <MemBufFormatTarget> ft;
			ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
			Task::Task (*ft, task, map);

			if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
				memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
				fifo_msg.size = ft->getLen();
				n=writeFifo(OUTBOARD_TX);
				if( n != (int)ft->getLen() && errno != EAGAIN)
					perror("DataToXML::update() - error writing to fifo ");
			}
			else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_FP_SUBS: {
			int nPoints = (cmd.size()/sizeof(Waypt_t));

			// our format of waypoints
			Waypt_t * waypoint = (Waypt_t *)dataPtr;
			Waypt_t myWaypoints[nPoints];

			FlightPlan::FlightPlan_type flight_plan(a_client->name.c_str());

			for(int i = 0; i < nPoints; i++) 
			{
				myWaypoints[i] = waypoint[i];
				int wpInd      = myWaypoints[i].number; 

				FlightPlan::Waypoint xml_waypoint;

				xml_waypoint.Latitude() = myWaypoints[i].pos.lat;
				xml_waypoint.Longitude() = myWaypoints[i].pos.lon;
				xml_waypoint.SequenceNumber() = wpInd;
				xml_waypoint.Next() = myWaypoints[i].next;
				xml_waypoint.OrbitRadius() = myWaypoints[i].orbitRad;

				flight_plan.Waypoint().push_back(xml_waypoint);
			}

			xml_schema::namespace_infomap map;
			map[""].name = "http://FlightPlan.CUIntegration.com";
			map[""].schema = "..\\Schemas\\CU\\flight_plan.xsd";

			auto_ptr <MemBufFormatTarget> ft;
			ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
			FlightPlan::FlightPlan (*ft, flight_plan, map);

			if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
				memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
				fifo_msg.size = ft->getLen();
				n=writeFifo(OUTBOARD_TX);
				if( n != (int)ft->getLen() && errno != EAGAIN)
					perror("DataToXML::update() - error writing to fifo ");
			}
			else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_LINK_STATUS: {
				int num_links = cmd.size()/sizeof(LinkStatus_t);

				LinkStatus_t * link;
				LinkStatus::LinkStatus_type link_status(a_client->name.c_str());

				for(int i=0;i<num_links;i++){
					link = &(((LinkStatus_t *)(dataPtr))[i]);

					char mac[18];
					sprintf(mac,"%02X:%02X:%02X:%02X:%02X:%02X",link->mac[0],link->mac[1],link->mac[2],link->mac[3],link->mac[4],link->mac[5]);
					char ip[16];
					sprintf(ip,"%d.%d.%d.%d",link->ip[0],link->ip[1],link->ip[2],link->ip[3]);

					LinkStatus::WifiComms wifi_comms;

					wifi_comms.MAC() = mac;
					wifi_comms.IP() = ip;
					wifi_comms.Signal() = link->signal;
					wifi_comms.Noise() = link->noise;
					wifi_comms.LastRx() = link->last_rx;
					wifi_comms.BytesTx() = link->numBytesTx;
					wifi_comms.BytesRx() = link->numBytesRx;
					wifi_comms.RateTx() = link->rateTx;
					wifi_comms.RateRx() = link->rateRx;
					wifi_comms.Capacity() = link->channelCap;

					link_status.WifiComms().push_back(wifi_comms);
				}

				xml_schema::namespace_infomap map;
				map[""].name = "http://LinkStatus.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\link_status.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				LinkStatus::LinkStatus (*ft, link_status, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			} break;
		case CMD_ACK: {
				TaskStatus::TaskStatus_type status(a_client->name.c_str(), TaskStatus::StatusType::Acknowledge);  

				xml_schema::namespace_infomap map;
				map[""].name = "http://TaskStatus.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\task_status.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				TaskStatus::TaskStatus (*ft, status, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			 } break;
		case CMD_FAIL: {
				TaskStatus::TaskStatus_type status(a_client->name.c_str(), TaskStatus::StatusType::Failed);  

				xml_schema::namespace_infomap map;
				map[""].name = "http://TaskStatus.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\task_status.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				TaskStatus::TaskStatus (*ft, status, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			 } break;
		case CMD_STREAM_REQ: {
				StreamReq_t request;
				CopyBufToStruct( dataPtr, request);

        Client * subscriber = shmPtr->clients.getClient(request.client);
        if(subscriber == NULL) break;
        Client * server = shmPtr->clients.getClient(fifo_msg.dest);
        if(server == NULL) break;

				StreamSubscribe::StreamSubscribe_type stream_request(
            server->name.c_str(),
            CUIntegration::StreamSubscribe::System(
              CUIntegration::StreamSubscribe::ID(""),
              CUIntegration::StreamSubscribe::Stream(""),
              CUIntegration::StreamSubscribe::Action("")));

        switch(request.id & 0xF0) {
          case CMD_SERVO:
            stream_request.System().ID("Servo");
            break;
          case CMD_IMU:
            stream_request.System().ID("IMU");
            switch(request.id) {
              case CMD_IMU_RATE:
                stream_request.System().Stream("Rates");
              case CMD_IMU_ANG:
                stream_request.System().Stream("Angles");
              case CMD_IMU_ACC:
                stream_request.System().Stream("Accelerations");
            }
            break;
          case CMD_AIR_DATA:
            stream_request.System().ID("Air Data");
            switch(request.id) {
              case CMD_WIND:
                stream_request.System().Stream("Wind");
            }
            break;
          case CMD_GPS:
            stream_request.System().ID("GPS");
            switch(request.id) {
              case CMD_GPS_LATLON:
                stream_request.System().Stream("Latitude and Longitude");
              case CMD_GPS_ALT:
                stream_request.System().Stream("Altitude");
              case CMD_GPS_UTC:
                stream_request.System().Stream("UTC");
              case CMD_GPS_TRACK:
                stream_request.System().Stream("Track");
              case CMD_GPS_SOG:
                stream_request.System().Stream("SOG");
              case CMD_GPS_ACCURACY:
                stream_request.System().Stream("Accuracy");
              case CMD_GPS_TELEM:
                stream_request.System().Stream("Telemetry");
            }
            break;
          case CMD_COMM:
            stream_request.System().ID("Communications");
            switch(request.id) {
              case CMD_LINK_STATUS:
                stream_request.System().Stream("Link Status");
            }
            break;
          case CMD_PAYLOAD:
            stream_request.System().ID("Payload");
            switch(request.id) {
              case CMD_PTH:
                stream_request.System().Stream("PTH");
                break;
              case CMD_SONDE:
                stream_request.System().Stream("Sonde");
                break;
            }
            break;
          case CMD_H_AND_S:
            stream_request.System().ID("Health and Status");
            break;
          case CMD_AUTOPILOT:
            stream_request.System().ID("Autopilot");
            switch(request.id) {
              case CMD_FP_SUBS:
                stream_request.System().Stream("Flight Plan");
                break;
              case CMD_TELEM:
                stream_request.System().Stream("Telemetry");
                break;
            }
            break;
          case CMD_FLIGHT_COMPUTER:
            stream_request.System().ID("Flight Computer");
            break;
        }

        if(request.send_data)
          stream_request.System().Action("Subscribe");
        else
          stream_request.System().Action("Unsubscribe");

				xml_schema::namespace_infomap map;
				map[""].name = "http://StreamSubscribe.CUIntegration.com";
				map[""].schema = "..\\Schemas\\CU\\stream_subscription.xsd";

				auto_ptr <MemBufFormatTarget> ft;
				ft = auto_ptr<MemBufFormatTarget> (new MemBufFormatTarget());
				StreamSubscribe::StreamSubscribe (*ft, stream_request, map);

				if(ft->getLen() <  (FIFO_BUF_SIZE - FIFO_HDR_SIZE)) {
					memcpy(fifo_msg.data, ft->getRawBuffer(), ft->getLen());
					fifo_msg.size = ft->getLen();
					n=writeFifo(OUTBOARD_TX);
					if( n != (int)ft->getLen() && errno != EAGAIN)
						perror("DataToXML::update() - error writing to fifo ");
				}
				else cout << "DataToXML::update() - error writing to fifo, size exceeds buffer size" << endl;

			 } break;*/
		default:
			break;
	}
}

DataToXML_Parser::~DataToXML_Parser()
{
	xercesc::XMLPlatformUtils::Terminate();
}
